#ifndef COSTMAP_GENERATER_CORE_H
#define COSTMAP_GENERATER_CORE_H

#include "ros/ros.h"
#include "nav_msgs/OccupancyGrid.h"
#include "sensor_msgs/PointCloud2.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "pcl_ros/transforms.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/passthrough.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#define __APP_NAME__ "costmap_generater"

class CostmapGenerater{
    public:
        CostmapGenerater();
        ~CostmapGenerater();
        void run();
    
    private:
        ros::NodeHandle nh_;
        ros::NodeHandle private_nh_;
        tf2_ros::Buffer buffer;
        tf2_ros::TransformListener tf2_listener;
    
    private:
        std::string output_costmap_frame_id;
        std::string output_costmap_topic;
        std::string input_confusion_pc2_topic;

    private:
        double costmap_resolution;
        int costmap_height;
        int costmap_width;
    
    private:
        sensor_msgs::PointCloud2 confusion_pointcloud2_;
        nav_msgs::OccupancyGrid initial_costmap_;
    
    private:
        ros::Subscriber confusion_pointcloud2_sub;
        ros::Publisher cost_map_pub;

    private:
        int obstacle_maxnum = 0;
        
    private:
        /* 融合点云回调函数 */
        void confusion_pointcloud2_call_back(const::sensor_msgs::PointCloud2::ConstPtr& msg);

        /* 判断点是否在地图内 */
        bool valid_point_in_map(double x, double y);

        /* 判断点在地图的索引值 */
        int get_index(double x, double y);

        /* 初始化栅格地图 一张白纸 */
        void init_cost_map();

        void process(const ros::TimerEvent& e);
};          

#endif